import processing.video.*;
import java.util.Collections;
import java.util.List;
import java.util.LinkedList;

Capture cam;
float totalDistTraveled = 0;
float inchesToPixels = 0;
Point actual;
Point relative;
Point origin;
PFont font;
Point blob1;
Point blob2;
Point previous;
Point calibration1;
Point calibration2;

int blobsAssigned;
int blobsMax;
float sensitivityFactor = 20;
//float calibrationFactor = 10;
float calibrationFactor = 7;
float blobOutlierFactor = 1.5;
int RUNNING_AVG_LENGTH = 30;
// calibrationFactor is the measured distance in inches between two 
// identifying marks on the robot chassis

// Variables from color detection code
int numPixels;                      // number of pixels in the video
int rectDivide = 1;                 // the stage width/height divided by this number is the video width/height 
int vidW;                           // video width
int vidH;                           // video height
int[][] colourCompareData;          // captured r, g and b colours
int currR;                          //
int currG;                          //
int currB;                          //
int[][] squareCoords;               // x, y, w + h of the coloured areas
color[] colours;                    // captured colours
int colourRange = 25;               // colour threshold
int[][] centrePoints;               // centres of the coloured squares
color[] pixelColours;
boolean isShowPixels = false;       // determines whether the square and coloured pixels are displayed
int colourMax = 2;                  // max amount of colours - also adjust the amount of colours added to pixelColours in setup()
int coloursAssigned = 0;            // amount of cours currently assigned
CoordsCalc coordsCalc;
LinkedList<Point> previousPoints;    // list of previous positions in order to compute a running mean


void setup() {
  size(1280, 720);
  initializeVariables();
  setupCamera(); // not using this setup method anymore
  setupDetection();
}

void initializeVariables() {
  actual = new Point(300, 200);
  relative = new Point(0, 0);
  origin = new Point(0, 0);
  blob1 = new Point(0, 0);
  blob2 = new Point(0, 0);
  previous = new Point(0, 0);
  calibration1 = new Point(0, 0);
  calibration2 = new Point(0, 0);
  font = createFont("Arial", 16, true);
  blobsAssigned = 0;
  blobsMax = 2;
  previousPoints = new LinkedList<Point>();
}

void setupCamera() {
  String[] cameras = Capture.list();

  if (cameras.length == 0) {
    println("There are no cameras available for capture.");
    exit();
  } 
  else {
        println("Available cameras:");
        for (int i = 0; i < cameras.length; i++) {
          println(cameras[i]);
        }

    // The camera can be initialized directly using an 
    // element from the array returned by list():
  }
}

void setupDetection() {
  vidW = width / rectDivide;
  vidH = height / rectDivide;
  // Find camera name from printed list. Make sure size and frame rate agree
//  cam = new Capture(this, vidW, vidH, "VF0610 Live! Cam Socialize HD", 10);
    cam = new Capture(this, vidW, vidH, "/dev/video1", 10);
  
  noStroke();
  numPixels = vidW * vidH;
  colourCompareData = new int[colourMax][3];
  squareCoords = new int[colourMax][4];
  colours = new color[colourMax];
  centrePoints = new int[colourMax][2];
  color c1 = color(0, 255, 0);
  color c2 = color(255, 0, 0);
  pixelColours = new color[colourMax];
  pixelColours[0] = color(0, 255, 0);
  pixelColours[1] = color(255, 0, 0);
  coordsCalc = new CoordsCalc();
  cam.start();
}

void captureEvent(Capture cam) { // unused, copied from color detection code
  cam.read();
}

void draw() {
  // updates color detection code
  updateDetection();
  updateBlobs();

  // model
  updateCameraStream();
  //  updateScale(); // scale is now updated when origin is reset
  updateActualLocation();
  updateRelativeLocation();
  updateTotalDistanceTraveled();

  // view
  //  displayImage(); // image is displayed in updateDetection();
  displayCurrentLocation();
  displayText();
}

void updateDetection() {
  noStroke();
  fill(255, 255, 255);
  rect(0, 0, width, height);
  drawVideo();
  coordsCalc.update();
  for (int i = 0; i < coloursAssigned; i++) {
    if (isShowPixels) drawSquare(i);
  }
}

void updateBlobs() {

  for (int i = 0; i < coloursAssigned; i++) {
    int sqX = squareCoords[i][0];
    int sqY = squareCoords[i][1];
    int sqW = squareCoords[i][2];
    int sqH = squareCoords[i][3];

    if (i == 0) {
      blob1.x = sqX + sqW / 2;
      blob1.y = sqY + sqH / 2;
//      System.out.println("x, y are: " + blob1.x + " and " + blob1.y  );
    }
    if (i == 1) {
      blob2.x = sqX + sqW / 2;
      blob2.y = sqY + sqH / 2;
//      System.out.println("x, y are: " + blob2.x + " and " + blob2.y  );
    }
  }
}

// resets the origin to current location
void resetOrigin() {
  origin.x = actual.x;
  origin.y = actual.y;
}

// updates the camera stream input (do first)
void updateCameraStream() {
  if (cam.available() == true) {
    cam.read();
  }
}


// updates the conversion factor based on distance of blobs
// do we want to update every update cycle?
void updateScale() {
  // update inchesToPixels
// distance between two calibration points is 7.357 inches
  inchesToPixels = abs(calibrationFactor /  dist(calibration1.x, calibration1.y, calibration2.x, calibration2.y));
}


// updates actual (x, y) of robot
void updateActualLocation() {
  
  while (previousPoints.size() >= RUNNING_AVG_LENGTH && previousPoints.size() > 1) {
      previousPoints.remove();
  }
    
  if (coloursAssigned == 1) {
    previousPoints.add(new Point(blob1));
    actual.set(findRunningAvg(previousPoints));
  }

  if (coloursAssigned == 2) {
    //midpoint between blobs
    int midX = (int) (blob1.x + blob2.x) / 2;
    int midY = (int) (blob1.y + blob2.y) / 2;
    previousPoints.add(new Point(midX, midY));
    actual.set(findRunningAvg(previousPoints));
  }
  
}

Point findRunningAvg(LinkedList<Point> prev) {
  int avgX = 0;
  int avgY = 0;
  int listLength = prev.size();
  for (int i = 0; i < listLength; i++) {
    avgX = avgX + prev.get(i).x;
    avgY = avgY + prev.get(i).y;
  }
//  System.out.println("Sum of X's: " + avgX);
  avgX = avgX/listLength;
  avgY = avgY/listLength;
  return new Point(avgX, avgY);
}

//TODO: does this work?
// updates 
void updateRelativeLocation() {
  relative.x = actual.x - origin.x;
  relative.y = actual.y - origin.y;
}

// TODO
// updates dist. traveled in inches; uses scale factor
void updateTotalDistanceTraveled() {
  // update totalDistTraveled
  float tempDelta = dist(actual.x, actual.y, previous.x, previous.y);
//  System.out.println("x= " + actual.x +  " y= " + actual.y);
//  System.out.println("px= " + previous.x +  " py= " + previous.y);
  // use sensitivity factor
  if (tempDelta > sensitivityFactor) {
    totalDistTraveled += (tempDelta*inchesToPixels);
    previous.x = actual.x;
    previous.y = actual.y;
  }

//  System.out.println("tempDelta= " + tempDelta);
//  System.out.println("total dist= " + totalDistTraveled);
}

// displays the camera image
void displayImage() {
  image(cam, 0, 0);
  // The following does the same, and is faster when just drawing the image
  // without any additional resizing, transformations, or tint.
  //  set(0, 0, cam);
}

// displays a rectangle around current location
void displayCurrentLocation() {
  float myWidth = 75; // width constant
  float myHeight = 75; // height constant

  noFill();
  stroke(204, 102, 0);
  rect(actual.x - myWidth/2, actual.y - myHeight/2, myWidth, myHeight);
}

// TODO
// displays text for relative (x, y) and total distance traveled
// and current scale (text along the bottom)
void displayText() {

  String distance = "Total Distance Traveled: " + totalDistTraveled;
  String relativeX = "X-coord (Relative to Origin): " + relative.x;
  String relativeY = "Y-coord (Relative to Origin): " + relative.y;
  String actualPoint = "Actual point (relative to top Left): " + actual.x + ", " + actual.y;
  float location = 630; // location on the screen

  textFont(font);       
  fill(0);

  textAlign(CENTER);
  fill(255);
  stroke(0);
  rect(width/2 - textWidth(distance)/2 - 5, location - 16, textWidth(distance) + 10, 20);
  fill(0);
  text(distance, width/2, location);
  fill(255);
  rect(width/2 - textWidth(relativeX)/2 - 5, location + 25 - 16, textWidth(relativeX) + 10, 20);
  fill(0);
  text(relativeX, width/2, location + 25); 
  fill(255);
  rect(width/2 - textWidth(relativeY)/2 - 5, location + 50 - 16, textWidth(relativeY) + 10, 20);
  fill(0);
  text(relativeY, width/2, location + 50);
  // output location of center of blobs
  fill(255);
  rect(width/2 - textWidth(actualPoint)/2 - 5, location + 75 - 16, textWidth(actualPoint) + 10, 20);
  fill(0);
  text(actualPoint, width/2, location + 75);
}


void drawVideo() {
  for (int i = 0; i < coloursAssigned; i++) {
    fill(colours[i]);
    rect(i * 10, vidH, 10, 10);
  }
  image(cam, 0, 0);
  noFill();
  stroke(255, 0, 0);
  strokeWeight(2);
}

void drawSquare(int i) {
  int sqX = squareCoords[i][0];
  int sqY = squareCoords[i][1];
  int sqW = squareCoords[i][2];
  int sqH = squareCoords[i][3];
  noFill();
  stroke(0, 0, 255);
  strokeWeight(3);
  rect(sqX, sqY, sqW, sqH);

  //stroke(0, 0, 255);
  //strokeWeight(4);
  rect(sqX * rectDivide, sqY * rectDivide, sqW * rectDivide, sqH * rectDivide);
  line(sqX * rectDivide, sqY * rectDivide, ((sqX * rectDivide) + (sqW * rectDivide)), ((sqY * rectDivide) + (sqH * rectDivide)));
  line(((sqX * rectDivide) + (sqW * rectDivide)), sqY * rectDivide, sqX * rectDivide, (sqY * rectDivide + sqH * rectDivide));
}

void mouseClicked() {
  color currPixColor = cam.pixels[mouseX + vidW * mouseY];
  int pixR = (currPixColor >> 16) & 0xFF;
  int pixG = (currPixColor >> 8) & 0xFF;
  int pixB = currPixColor & 0xFF;

  if (coloursAssigned == 0) {
    coloursAssigned = 1;
    colourCompareData[0][0] = pixR;
    colourCompareData[0][1] = pixG;
    colourCompareData[0][2] = pixB;
    colours[0] = color(pixR, pixG, pixB);
    return;
  }
  if (colourMax < 2 || coloursAssigned < 1) return;
  if (coloursAssigned == 1) {
    coloursAssigned = 2;
    colourCompareData[1][0] = pixR;
    colourCompareData[1][1] = pixG;
    colourCompareData[1][2] = pixB;
    colours[1] = color(pixR, pixG, pixB);
    return;
  }
  if (coloursAssigned == 2) {
    coloursAssigned = 0;
  }
}

void keyPressed() {
  println("key pressed = " + key);

  if (key == 'p') {
    isShowPixels = !isShowPixels;
  }

  if (key == '0') {
    coloursAssigned = 0;
  }
if (key == 'c') {
 totalDistTraveled = 0; 
}
  if (key == 'r') {
    updateScale();
    resetOrigin();
  }
  // key press used to set marker location on robot
  if (key == 'b') {

    if (blobsAssigned == 0) {
      calibration1.x = mouseX;
      calibration1.y = mouseY;
      blobsAssigned = 1;
      //System.out.println("assigned b1");
      return;
    }
    if (blobsMax < 2 || blobsAssigned < 1) return;
    if (blobsAssigned == 1) {
      blobsAssigned = 2;
      calibration2.x = mouseX;
      calibration2.y = mouseY;
      //System.out.println("assigned b2");
      // re-calibrate
      updateScale();
      blobsAssigned = 0;
      // reset distance traveled to zero, now that markers identified
      totalDistTraveled = 0;
//      System.out.println("dist = 0");
      updateActualLocation();
      blobsAssigned = 0;
      return;
    }
    //    if (blobsAssigned == 2) {
    //     // re-calibrate
    //     updateScale();
    //     blobsAssigned = 0;
    //     // reset distance traveled to zero, now that markers identified
    //     totalDistTraveled = 0;
    //     System.out.println("dist = 0");
    //     updateActualLocation();
    //    }
  }
}

